#include "common.hpp"

namespace str
{
    void replace_char(std::string& line, const char old, const char rep)
    {
        std::string::iterator it;
        for (it = line.begin(); it < line.end(); it++)
        {
            if (*it == old)
            {
                line.erase(it);
                line.insert(it, rep);
                it--;
            }
        }
    }
}

namespace file
{
    void read_cloud(const std::string& file_path, PointCloud& cloud, const bool read_rgb)
    {
        Rgb color;
        Vec3d point;
        std::string line;
        cloud.clear();
        std::ifstream file(file_path.c_str());
        std::stringstream ss;

        while (std::getline(file, line))
        {
            ss.clear();
            ss.str("");
            ss << line;
            ss >> point.x;
            ss >> point.y;
            ss >> point.z;
            std::cout<<point.x<<","<<point.y<<","<<point.z<<std::endl;
            if (read_rgb) {
                ss >> color.r;
                ss >> color.g;
                ss >> color.b;
                cloud.colors.push_back(color);
            }
            if (cloud.size() == 0) {
                cloud.set_min(point);
                cloud.set_max(point);
            }
            else {
                cloud.update_min(point);
                cloud.update_max(point);
            }
            cloud.update_avg(point);
            cloud.points.push_back(point);
        }
        cloud.point_mid.x = (cloud.point_max.x + cloud.point_min.x) / 2;
        cloud.point_mid.y = (cloud.point_max.y + cloud.point_min.y) / 2;
        cloud.point_mid.z = (cloud.point_max.z + cloud.point_min.z) / 2;

        cloud.point_avg.x = cloud.point_avg.x / cloud.size();
        cloud.point_avg.y = cloud.point_avg.y / cloud.size();
        cloud.point_avg.z = cloud.point_avg.z / cloud.size();

        file.close();
    }
}

namespace pt
{
    void update_min(Vec3d& point_min, const Vec3d& point)
    {
        if (point.x < point_min.x) {
            point_min.x = point.x;
        }
        if (point.y < point_min.y) {
            point_min.y = point.y;
        }
        if (point.z < point_min.z) {
            point_min.z = point.z;
        }
    }

    void update_max(Vec3d& point_max, const Vec3d& point)
    {
        if (point.x > point_max.x) {
            point_max.x = point.x;
        }
        if (point.y > point_max.y) {
            point_max.y = point.y;
        }
        if (point.z > point_max.z) {
            point_max.z = point.z;
        }
    }

    Vec3d get_dimen(const Vec3d& point_min, const Vec3d& point_max)
    {
        Vec3d tp = { 
            std::fabs(point_max.x - point_min.x),
            std::fabs(point_max.y - point_min.y),
            std::fabs(point_max.z - point_min.z) 
        };
        return tp;
    }

    void point2_box_arr(const Vec3d& point, const Vec3d& dimen, float box[24])
    {
        box[0] = point.x;
        box[1] = point.y;
        box[2] = point.z;

        box[3] = point.x + dimen.x;
        box[4] = point.y;
        box[5] = point.z;

        box[6] = point.x + dimen.x;
        box[7] = point.y + dimen.y;
        box[8] = point.z;

        box[9] = point.x;
        box[10] = point.y + dimen.y;
        box[11] = point.z;

        box[12] = point.x;
        box[13] = point.y;
        box[14] = point.z + dimen.z;

        box[15] = point.x + dimen.x;
        box[16] = point.y;
        box[17] = point.z + dimen.z;

        box[18] = point.x + dimen.x;
        box[19] = point.y + dimen.y;
        box[20] = point.z + dimen.z;

        box[21] = point.x;
        box[22] = point.y + dimen.y;
        box[23] = point.z + dimen.z;
    }

    void point2_box(const Vec3d& point, const Vec3d& dimen, std::array<Vec3d, 8>& box)
    {
        box[0] = { point.x ,point.y,point.z };
        box[1] = { point.x + dimen.x,point.y,point.z };
        box[2] = { point.x + dimen.x,point.y + dimen.y,point.z };
        box[3] = { point.x,point.y + dimen.y,point.z };

        box[4] = { point.x ,point.y,point.z + dimen.z };
        box[5] = { point.x + dimen.x,point.y,point.z + dimen.z };
        box[6] = { point.x + dimen.x,point.y + dimen.y,point.z + dimen.z };
        box[7] = { point.x,point.y + dimen.y,point.z + dimen.z };
    }
}

PointCloud::PointCloud()
{
    clear();
}

PointCloud::~PointCloud()
{
    clear();
}

void PointCloud::set_min(const Vec3d& point)
{
    point_min.x = point.x;
    point_min.y = point.y;
    point_min.z = point.z;
}

void PointCloud::set_max(const Vec3d& point)
{
    point_max.x = point.x;
    point_max.y = point.y;
    point_max.z = point.z;
}

void PointCloud::update_min(const Vec3d& point)
{
    if (point.x < point_min.x) {
        point_min.x = point.x;
    }
    if (point.y < point_min.y) {
        point_min.y = point.y;
    }
    if (point.z < point_min.z) {
        point_min.z = point.z;
    }
}

void PointCloud::update_max(const Vec3d& point)
{
    if (point.x > point_max.x) {
        point_max.x = point.x;
    }
    if (point.y > point_max.y) {
        point_max.y = point.y;
    }
    if (point.z > point_max.z) {
        point_max.z = point.z;
    }
}

void PointCloud::update_avg(const Vec3d& point)
{
    point_avg.x += point.x;
    point_avg.y += point.y;
    point_avg.z += point.z;
}

int PointCloud::size()
{
    return points.size();
}

void PointCloud::clear()
{
    point_min = { 0,0,0 };
    point_max = { 0,0,0 };
    point_avg = { 0,0,0 };
    point_mid = { 0,0,0 };
    colors.clear();
    points.clear();
}